#include "../_includes.h"
#include "../globals/globals.h"
#include "../pid/pid.h"

Vis_Data vis_get_lowest_cdx(vision &v, signature &sig) {
  int dx, dy;
  v.takeSnapshot(sig);

  int k_lowest = 0;
  if (v.objectCount > 0) {
    if (&sig == &vis_BLUE)
      v.setLedColor(0, 0, 255);
    else if (&sig == &vis_RED)
      v.setLedColor(255, 0, 0);
    else if (&sig == &vis_GREEN)
      v.setLedColor(0, 255, 0);
    else if (&sig == &vis_YELLOW)
      v.setLedColor(255, 255, 0);

    for (auto k = 0; k < v.objectCount; k++) {
      if (k > 0) {  // 寻找视觉中最低的标签下标
        if (vis.objects[k].centerY > vis.objects[k_lowest].centerY)
          k_lowest = k;
      }
    }
    dx = vis.objects[k_lowest].centerX - VIS_MAX_X / 2.0;
    dy = vis.objects[k_lowest].centerY - VIS_MAX_Y / 2.0;
  } else {
    v.setLedColor(15, 25, 15);  // 微弱白光
    dx = 0;
    dy = 0;
  }
  wait(1);
  return Vis_Data(dx, dy);
}

void vis_forward_dist_relative(vision &v, signature &sig, int dist, int timeout,
                               double max_turn, double max_fwd) {
  pid_ctrl pid_turn =
      pid_ctrl::get_instance_old_version(0.8, 0.01, 0, 5, 0, 15, 10, max_turn);
  pid_ctrl pid_max_fwd =
      pid_ctrl::get_instance_old_version(1.5, 0, 0, 5, 0, 15, 10, max_fwd);

  timer t;
  double pturn;
  double pfwd;

  int dist_begin = chassis.get_dist_mm(-1);
  int dx_last = 0;
  bool data_ok;
  while (t.time() < timeout) {
    data_ok = false;
    Vis_Data data = vis_get_lowest_cdx(v, sig);
    cout << data;  // 打印

    // noise
    if (abs(data.dx) < 5) data.dx = 0;
    if (abs(data.dx) > 200) {
      cout << "dx error too large:" << data.dx << endl;
      data.dx = 0;
    } else {
      data_ok = true;
      cout << "dx ok:" << data.dx << endl;
    }
    if (abs(data.dx - dx_last) > 60) data.dx = 0;

    if (chassis.get_dist_mm(-1) - dist_begin > dist * 0.9) {
      data.dx = 0;
    }

    // 旋转纠正
    pturn = -pid_turn.pid_output_need_value(0, data.dx);
    // 前进降速
    max_fwd = pid_max_fwd.pid_output_need_err(85 - data.dy);  // 走到底座,dy为75
    pfwd = max_fwd - abs(pturn);  // 最大前进速度
    if (pfwd < 15) pfwd = 15;

    chassis.FT_set_voltage(pfwd, pturn);
    if (data_ok) dx_last = data.dx;

    wait(1);
    ///////////////////////
    if (chassis.get_dist_mm(-1) - dist_begin > dist) break;
    cout << endl;
    wait(2);
  }
  chassis.stop();
}
